
/**
 *共享控制
 */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include "turtlesim/Pose.h"
#include <math.h>

static double x1,yl,yaw1,pitch1,roll1,     x2,y2,yaw2,pitch2,roll2,
         	                dx,dy,dyaw       ,dpitch,droll, yaw, pitch, roll,
						    ex,ey,eyaw,         ex_t,ey_t,eyaw_t,
          	                 v1,omega1,v2,omega2,
         	                 K1,K2,K3;


static ros::Subscriber sub1;
static ros::Subscriber sub2;
static ros::Publisher turtle_vel;

void doPose1(const geometry_msgs::Twist::ConstPtr    & p){
	x1=p->linear.x;
	omega1=p->angular.z;
}

void doPose2(const geometry_msgs::Twist::ConstPtr& p){
	x2=p->linear.x;
	omega2=p->angular.z;
}

// 方法一：订阅在发布中运行

int main(int argc, char** argv)
{
	setlocale(LC_ALL,"");

	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	sub1 = node.subscribe<geometry_msgs::Twist>("/turtle1/cmd_vel",1000,doPose1);
	sub2 = node.subscribe<geometry_msgs::Twist>("/turtle2/cmd_vel",1000,doPose2);
	// 创建发布turtle2速度控制指令的发布者
	turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle4/cmd_vel", 10);

	ros::Rate rate(10);
	while (node.ok())
	{

		dx=x1-x2;
		dy=yl-y2;
		dyaw=yaw1-yaw2;
        ex_t=cos(yaw2)*dx+sin(yaw2)*dy;
        ey_t=-sin(yaw2)*dx+cos(yaw2)*dy;
        eyaw_t=dyaw;


		// 根据turtle1与turtle3坐标系之间的位置关系，发布turtle2的速度控制指令
		geometry_msgs::Twist vel_msg;

        K1=0.909;
        K2=1.250;
        K3=0.818;

		vel_msg.linear.x = x1+x2;
		vel_msg.angular.z = omega1+omega2;

		ROS_INFO("乌龟1运动信息:lv=%.2f,av=%.2f",x1,omega1);
		ROS_INFO("乌龟2运动信息:lv=%.2f,av=%.2f",x2,omega2);
		ROS_INFO("乌龟4运动信息:lv=%.2f,av=%.2f",vel_msg.linear.x,vel_msg.angular.z);

		turtle_vel.publish(vel_msg);

		rate.sleep();
		ros::spinOnce();
	}
	return 0;
};
